#include <stdio.h>
#include <math.h>
#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include <cuda_runtime.h>
// #include "cuda_cal.h"

struct PointResult{
  double* azimuth;
  double* pitch;
  double* distance;
};

struct PointXYZI_Float
{
  float x;
  float y;
  float z;
  float intensity;
};

// 1. CPU内存
static PointXYZI_Float* h_points;
static double* cpu_azimuth;
static double* cpu_pitch;
static double* cpu_distance;
static PointResult cpu_point_result;
// 3.总个数
static uint32_t total;

PointResult* cpu_cal(const PointXYZI_Float* points, double* azimuth, double* pitch, double* distance, int total)
{
    for (int idx = 0;idx < total;idx++) {
        const PointXYZI_Float& p = points[idx];

        // 使用CUDA设备端数学函数
        azimuth[idx] = atan2f(p.y, p.x) * 57.2958;
        float r = p.x * p.x + p.y * p.y;
        pitch[idx] = atan2f(p.z, sqrtf(r)) * 57.2958;
        distance[idx] = sqrtf(r + p.z * p.z);
    }
    cpu_point_result.azimuth = azimuth;
    cpu_point_result.pitch = pitch;
    cpu_point_result.distance = distance;
    return &cpu_point_result;
}

bool init_cpu_mem()
{
    const uint32_t width = 1800;
    const uint32_t height = 32 * 2;
    total = width * height;

    h_points = (struct PointXYZI_Float *)malloc(total * sizeof(PointXYZI_Float));
    cpu_azimuth = (double *)malloc(total * sizeof(double));
    cpu_pitch = (double *)malloc(total * sizeof(double));
    cpu_distance = (double *)malloc(total * sizeof(double));
    if(!h_points || !cpu_azimuth || !cpu_pitch || !cpu_distance)
    {
        fprintf(stderr, "CPU内存初始化失败\n");
        return false;
    }

    return true;
}

void release_cpu_mem()
{
    free(cpu_azimuth);
    free(cpu_pitch);
    free(cpu_distance);
    free(h_points);
}

void cpu_test()
{
    if(!init_cpu_mem())
    {
        fprintf(stderr, "GPU内存初始化失败\n");
        return;
    }

    // 初始化点数据
    for (uint32_t i = 0; i < total; i++) {
        h_points[i].x = 1.0f;
        h_points[i].y = 2.0f;
        h_points[i].z = 3.0f;
    }
    PointResult* cpu_result = cpu_cal(h_points,cpu_azimuth,cpu_pitch,cpu_distance,total);

    // 验证结果
    printf("第一个点的计算结果:\n");
    printf("azimuth: %f\n", cpu_result->azimuth[0]);
    printf("pitch: %f\n", cpu_result->pitch[0]);
    printf("distance: %f\n", cpu_result->distance[0]);

    release_cpu_mem();
}

#if 1
int main()
{
    cpu_test();
    return 0;
}
#endif
